#include "MMath.h"
#include <cmath>
#include <float.h>
#include <cstring>


//////////////////////////////////////
// Vec2
//////////////////////////////////////
Vec2 Vec2::operator+(Vec2 &v)
{
	return Vec2(x + v.x, y + v.y);
}
Vec2 Vec2::operator-(Vec2 &v)
{
	return Vec2(x - v.x, y - v.y);
}
Vec2 Vec2::operator*(float f)
{
	return Vec2(x * f, y * f);
}
Vec2 Vec2::operator/(float f)
{
	return Vec2(x / f, y / f);
}
Vec2 Vec2::operator+=(Vec2 &v)
{
	x += v.x;
	y += v.y;
	return *this;
}
Vec2 Vec2::operator-=(Vec2 &v)
{
	x -= v.x;
	y -= v.y;
	return *this;
}
Vec2 Vec2::operator*=(float f)
{
	x *= f;
	y *= f;
	return *this;
}
Vec2 Vec2::operator/=(float f)
{
	x /= f;
	y /= f;
	return *this;
}
Vec2 Vec2::operator-()
{
	return Vec2(-x, -y);
}
bool Vec2::operator!=(Vec2 &v)
{
	if (x != v.x || y != v.y)
		return true;
	return false;
}
bool Vec2::operator==(Vec2 &v)
{
	if (x == v.x && y == v.y)
		return true;
	return false;
}

void Vec2::Negate()
{
	x = -x;
	y = -y;
}
float Vec2::LengthSq()
{
	return x * x + y * y;
}
float Vec2::Length()
{
	return sqrt(x * x + y * y);
}
void Vec2::Normalize()
{
	float len = Length();
	operator/=(len);
}

float AngleBetweenR(Vec2 &v1, Vec2 &v2)
{
	float len1 = v1.Length();
	float len2 = v2.Length();
	if (!len1 || !len2)
		return 0;
	float angle = acosf(Dot(v1, v2) / (len1 * len2));
	if (_isnan(angle))
		return 0;
	return angle;
}
float AngleBetweenD(Vec2 &v1, Vec2 &v2)
{
	return AngleBetweenR(v1, v2) * 180.0f / 3.14159f;
}
float Dot(Vec2 &v1, Vec2 &v2)
{
	return v1.x * v2.x + v1.y * v2.y;
}
float Component(Vec2 &v1, Vec2 &v2)
{
	// if either is zero vector, return zero
	float len1 = v1.Length();
	float len2 = v2.Length();
	if (!len1 || !len2)
		return 0;
	return Dot(v1, v2) / len2;
}
Vec2 Project(Vec2 &v1, Vec2 &v2)
{
	float len1 = v1.Length();
	float len2 = v2.Length();
	if (!len1 || !len2)
		return Vec2(0, 0);
	float comp = Dot(v1, v2) / (len2 * len2);
	return v2 * comp;
}
Vec2 Reflect(Vec2 &v, Vec2 &n)
{
	float len1 = v.Length();
	float len2 = n.Length();
	if (!len1 || !len2)
		return Vec2(0, 0);

	float comp = Dot(v, n) / (len2 * len2);
	return (n * comp * 2) - v;
}

//////////////////////////////////////
// Vec3
//////////////////////////////////////
Vec3 Vec3::operator+(Vec3 &v)
{
	return Vec3(x + v.x, y + v.y, z + v.z);
}
Vec3 Vec3::operator-(Vec3 &v)
{
	return Vec3(x - v.x, y - v.y, z - v.z);
}
Vec3 Vec3::operator*(float f)
{
	return Vec3(x * f, y * f, z * f);
}
Vec3 Vec3::operator/(float f)
{
	return Vec3(x / f, y / f, z / f);
}
Vec3 Vec3::operator+=(Vec3 &v)
{
	x += v.x; y += v.y; z += v.z;
	return *this;
}
Vec3 Vec3::operator-=(Vec3 &v)
{
	x -= v.x; y -= v.y; z -= v.z;
	return *this;
}
Vec3 Vec3::operator*=(float f)
{
	x *= f; y *= f; z *= f;
	return *this;
}
Vec3 Vec3::operator/=(float f)
{
	x /= f; y /= f; z /= f;
	return *this;
}
Vec3 Vec3::operator-()
{
	return Vec3(-x, -y, -z);
}
bool Vec3::operator!=(Vec3 &v)
{
	if (x == v.x && y == v.y && z == v.z)
		return false;
	return true;
}
bool Vec3::operator==(Vec3 &v)
{
	if (x == v.x && y == v.y && z == v.z)
		return true;
	return false;
}

void Vec3::Negate()
{
	x = -x; y = -y; z = -z;
}
float Vec3::LengthSq()
{
	return x * x + y * y + z * z;
}
float Vec3::Length()
{
	return sqrt(LengthSq());
}
void Vec3::Normalize()
{
	operator/=(Length());
}

float AngleBetweenR(Vec3 &v1, Vec3 &v2)
{
	float len1 = v1.Length();
	float len2 = v2.Length();
	if (!len1 || !len2)
		return 0;
	float angle = acosf(Dot(v1, v2) / (len1 * len2));
	if (_isnan(angle))
		return 0;
	return angle;
}
float AngleBetweenD(Vec3 &v1, Vec3 &v2)
{
	return AngleBetweenR(v1, v2) * 180.0f / 3.14159f;
}
float Dot(Vec3 &v1, Vec3 &v2)
{
	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
float Component(Vec3 &v1, Vec3 &v2)
{
	float len1 = v1.Length();
	float len2 = v2.Length();
	if (!len1 || !len2)
		return 0;
	return Dot(v1, v2) / len2;
}
Vec3 Cross(Vec3 &v1, Vec3 &v2)
{
	return Vec3(v1.y * v2.z - v1.z * v2.y,
				v1.z * v2.x - v1.x * v2.z,
				v1.x * v2.y - v1.y * v2.x);
}
Vec3 Project(Vec3 &v1, Vec3 &v2)
{
	float len1 = v1.Length();
	float len2 = v2.Length();
	if (!len1 || !len2)
		return Vec3(0, 0, 0);
	float comp = Dot(v1, v2) / (len2 * len2);
	return v2 * comp;
}
Vec3 Reflect(Vec3 &v, Vec3 &n)
{
	float len1 = v.Length();
	float len2 = n.Length();
	if (!len1 || !len2)
		return Vec3(0, 0, 0);

	float comp = Dot(v, n) / (len2 * len2);
	return (n * comp * 2) - v;
}

//////////////////////////////////////
// Mat3
//////////////////////////////////////
Mat3 Mat3::operator*(Mat3 &m)
{
	return Mat3(xx * m.xx + xy * m.yx + xz * m.zx,
				xx * m.xy + xy * m.yy + xz * m.zy,
				xx * m.xz + xy * m.yz + xz * m.zz,
				yx * m.xx + yy * m.yx + yz * m.zx,
				yx * m.xy + yy * m.yy + yz * m.zy,
				yx * m.xz + yy * m.yz + yz * m.zz,
				zx * m.xx + zy * m.yx + zz * m.zx,
				zx * m.xy + zy * m.yy + zz * m.zy,
				zx * m.xz + zy * m.yz + zz * m.zz);
}
Vec2 Mat3::operator*(Vec2 &v)
{
	return Vec2(xx * v.x + xy * v.y + xz,
				yx * v.x + yy * v.y + yz);//,
				//wx * v.x + wy * v.y + wz * v.z + ww);
}
Mat3 Mat3::operator*=(Mat3 &m)
{
	*this = operator*(m);
	return *this;
}
Mat3 Mat3::operator-()
{
	return Mat3(-xx, -xy, -xz,
				-yx, -yy, -yz,
				-zx, -zy, -zz);
}

Vec2 operator*(Vec2 &v, Mat3 &m)
{
	return Vec2(m.xx * v.x + m.yx * v.y + m.zx,
				m.xy * v.x + m.yy * v.y + m.zy);//,
				//m.xw * v.x + m.yw * v.y + m.zw * v.z + m.ww);
}

void Mat3::MakeZero()
{
	mm[0] = mm[1] = mm[2] = 0;
	mm[3] = mm[4] = mm[5] = 0;
	mm[6] = mm[7] = mm[8] = 0;
}
void Mat3::MakeIdentity()
{
	mm[0] = 1;
	mm[1] = mm[2] =	mm[3] = 0;
	mm[4] = 1;
	mm[5] = mm[6] = mm[7] = 0;
	mm[8] = 1;
}
void Mat3::Negate()
{
	for (int i = 0; i < 9; i++)
		mm[i] = -mm[i];
}
void Mat3::Transpose()
{
	Swap(xy, yx);
	Swap(xz, zx);
	Swap(yz, zy);
}
//void Mat3::Invert()
//{
//	*this = Mat3(xx, yx, zx, 0,	xy, yy, zy, 0, xz, yz, zz, 0,
//				-(wx * xx + wy * xy + wz * xz), -(wx * yx + wy * yy + wz * yz),
//				-(wx * zx + wy * zy + wz * zz), 1);
//}
void Mat3::MakeRotate(float angle)
{
	float sd = sin(angle);
	float cd = cos(angle);
	*this = Mat3(cd, -sd, 0,
				 sd, cd, 0,
				 0, 0, 1);
}
void Mat3::MakeTranslate(Vec2 &trans)
{
	*this = Mat3(1, 0, 0,
				 0, 1, 0,
				 trans.x, trans.y, 1);
}
void Mat3::MakeScale(Vec2 &scale)
{
	*this = Mat3(scale.x, 0, 0,
				 0, scale.y, 0,
				 0, 0, 1);
}

Mat3 Mat3::GetIdentity()
{
	Mat3 m;
	m.MakeIdentity();
	return m;
}
Mat3 Mat3::GetRotate(float angle)
{
	Mat3 m;
	m.MakeRotate(angle);
	return m;
}
Mat3 Mat3::GetTranslate(Vec2 &trans)
{
	Mat3 m;
	m.MakeTranslate(trans);
	return m;
}
Mat3 Mat3::GetScale(Vec2 &scale)
{
	Mat3 m;
	m.MakeScale(scale);
	return m;
}

//////////////////////////////////////
// Mat4
//////////////////////////////////////
Mat4 Mat4::operator*(Mat4 &m)
{
	return Mat4(xx * m.xx + xy * m.yx + xz * m.zx + xw * m.wx,
				xx * m.xy + xy * m.yy + xz * m.zy + xw * m.wy,
				xx * m.xz + xy * m.yz + xz * m.zz + xw * m.wz,
				xx * m.xw + xy * m.yw + xz * m.zw + xw * m.ww,

				yx * m.xx + yy * m.yx + yz * m.zx + yw * m.wx,
				yx * m.xy + yy * m.yy + yz * m.zy + yw * m.wy,
				yx * m.xz + yy * m.yz + yz * m.zz + yw * m.wz,
				yx * m.xw + yy * m.yw + yz * m.zw + yw * m.ww,

				zx * m.xx + zy * m.yx + zz * m.zx + zw * m.wx,
				zx * m.xy + zy * m.yy + zz * m.zy + zw * m.wy,
				zx * m.xz + zy * m.yz + zz * m.zz + zw * m.wz,
				zx * m.xw + zy * m.yw + zz * m.zw + zw * m.ww,
				
				wx * m.xx + wy * m.yx + wz * m.zx + ww * m.wx,
				wx * m.xy + wy * m.yy + wz * m.zy + ww * m.wy,
				wx * m.xz + wy * m.yz + wz * m.zz + ww * m.wz,
				wx * m.xw + wy * m.yw + wz * m.zw + ww * m.ww);
}
Vec3 Mat4::operator*(Vec3 &v)
{
	return Vec3(xx * v.x + xy * v.y + xz * v.z + xw,
				yx * v.x + yy * v.y + yz * v.z + yw,
				zx * v.x + zy * v.y + wz * v.z + zw);
}
Mat4 Mat4::operator*=(Mat4 &m)
{
	*this = operator*(m);
	return *this;
}

Mat4::Mat4(const D3DXMATRIX &mat)
{
	*this = mat;
}

Mat4 Mat4::operator=(const D3DXMATRIX &mat)
{
	xx = mat._11;
	xy = mat._21;
	xz = mat._31;
	xw = mat._41;

	yx = mat._12;
	yy = mat._22;
	yz = mat._32;
	yw = mat._42;

	zx = mat._13;
	zy = mat._23;
	zz = mat._33;
	zw = mat._43;

	wx = mat._14;
	wy = mat._24;
	wz = mat._34;
	ww = mat._44;

	return *this;
}

Mat4 Mat4::operator-()
{
	return Mat4(-xx, -xy, -xz, -xw,
				-yx, -yy, -yz, -yw,
				-zx, -zy, -zz, -zw,
				-wx, -wy, -wz, -ww);
}

Vec3 operator*(Vec3 &v, Mat4 &m)
{
	return Vec3(m.xx * v.x + m.yx * v.y + m.zx * v.z + m.wx,
				m.xy * v.x + m.yy * v.y + m.zy * v.z + m.wy,
				m.xw * v.x + m.yw * v.y + m.zw * v.z + m.ww);
}

void Mat4::MakeZero()
{
	memset(mm, 0, 64);
}
void Mat4::MakeIdentity()
{
	MakeZero();
	m[0][0] = m[1][1] = m[2][2] = m[3][3] = 1;
}
void Mat4::Negate()
{
	for (int i = 0; i < 16; i++)
		mm[i] = -mm[i];
}
void Mat4::Transpose()
{
	Swap(xy, yx);
	Swap(xz, zx);
	Swap(yz, zy);
	Swap(xw, wx);
	Swap(yw, wy);
	Swap(zw, wz);
}
void Mat4::OrthoNormalInvert()
{
	float fxw = -Dot(xaxis, waxis);
	float fyw = -Dot(yaxis, waxis);
	float fzw = -Dot(zaxis, waxis);
	Transpose();
	xw = yw = zw = 0;
	wx = fxw;
	wy = fyw;
	wz = fzw;
}
void Mat4::MakeRotateX(float angle)
{
	float sd = sin(angle);
	float cd = cos(angle);
	*this = Mat4(1, 0, 0, 0,
				 0, cd, -sd, 0,
				 0, sd, cd, 0,
				 0, 0, 0, 1);
}
void Mat4::MakeRotateY(float angle)
{
	float sd = sin(angle);
	float cd = cos(angle);
	*this = Mat4(cd, 0, sd, 0,
				 0, 1, 0, 0,
				 -sd, 0, cd, 0,
				 0, 0, 0, 1);
}
void Mat4::MakeRotateZ(float angle)
{
	float sd = sin(angle);
	float cd = cos(angle);
	*this = Mat4(cd, -sd, 0, 0,
				 sd, cd, 0, 0,
				 0, 0, 1, 0,
				 0, 0, 0, 1);
}
void Mat4::MakeRotateArbitrary(float angle, Vec3 &axis)
{
	float sd = sin(angle);
	float cd = cos(angle);
	*this = Mat4(axis.x * axis.x + (1 - axis.x * axis.x) * cd, axis.x * axis.y * (1 - cd) - axis.z * sd, axis.x * axis.z * (1 - cd) + axis.y * sd, 0,
				 axis.x * axis.y * (1 - cd) + axis.z * sd, axis.y * axis.y + (1 - axis.y * axis.y) * cd, axis.y * axis.z * (1 - cd) - axis.x * sd, 0,
				 axis.x * axis.z * (1 - cd) + axis.y * sd, axis.y * axis.z * (1 - cd) + axis.x * sd, axis.z * axis.z + (1 - axis.z * axis.z) * cd, 0,
		0, 0, 0, 1);
}
void Mat4::MakeTranslate(Vec3 &trans)
{
	*this = Mat4(1, 0, 0, 0,
				 0, 1, 0, 0,
				 0, 0, 1, 0,
				 trans.x, trans.y, trans.z, 1);
}
void Mat4::MakeScale(Vec3 &scale)
{
	*this = Mat4(scale.x, 0, 0, 0,
				 0, scale.y, 0, 0,
				 0, 0, scale.z, 0,
				 0, 0, 0, 1);
}

D3DXMATRIX Mat4::AsD3DXMatrix(void)
{
	D3DXMATRIX mat;

	mat._11	= xx;
	mat._21 = xy;
	mat._31 = xz;
	mat._41 = xw;

	mat._12 = yx;
	mat._22 = yy;
	mat._32 = yz;
	mat._42 = yw;

	mat._13 = zx;
	mat._23 = zy;
	mat._33 = zz;
	mat._43 = zw;

	mat._14 = wx;
	mat._24 = wy;
	mat._34 = wz;
	mat._44 = ww;

	return mat;
}

Mat4 Mat4::GetIdentity()
{
	Mat4 out;
	out.MakeIdentity();
	return out;
}
Mat4 Mat4::GetRotateX(float angle)
{
	Mat4 out;
	out.MakeRotateX(angle);
	return out;
}
Mat4 Mat4::GetRotateY(float angle)
{
	Mat4 out;
	out.MakeRotateY(angle);
	return out;
}
Mat4 Mat4::GetRotateZ(float angle)
{
	Mat4 out;
	out.MakeRotateZ(angle);
	return out;
}
Mat4 Mat4::GetRotateArbitrary(float angle, Vec3 &axis)
{
	Mat4 out;
	out.MakeRotateArbitrary(angle, axis);
	return out;
}
Mat4 Mat4::GetTranslate(Vec3 &trans)
{
	Mat4 out;
	out.MakeTranslate(trans);
	return out;
}
Mat4 Mat4::GetScale(Vec3 &scale)
{
	Mat4 out;
	out.MakeScale(scale);
	return out;
}

//////////////////////////////////////
// Quaternion
//////////////////////////////////////
Quaternion::Quaternion(float fAngle, Vec3 &Axis)
{
	float fA = fAngle * 0.5f;
	w = float(cos(fA));
	v = Axis * float(sin(fA));
}
Quaternion::Quaternion(const Mat4 &Mat)
{
	// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
	// article "Quaternion Calculus and Fast Animation".
	float fTrace = Mat.xx + Mat.yy + Mat.zz;
	if (fTrace > 0)
	{
		float fRoot = sqrt(fTrace + 1);
		w = 0.5f * fRoot;
		fRoot = 0.5f / fRoot;
		x = (Mat.zy - Mat.yz) * fRoot;
		y = (Mat.xz - Mat.zx) * fRoot;
		z = (Mat.yx - Mat.xy) * fRoot;
	}
	else
	{
		int nNext[3] = { 1, 2, 0 };
		int i = 0;
		if (Mat.yy > Mat.xx)
			i = 1;
		if (Mat.zz > Mat.m[i][i])
			i = 2;
		int j = nNext[i];
		int k = nNext[j];

		float fRoot = sqrt(Mat.m[i][i] - Mat.m[j][j] - Mat.m[k][k] + 1);
		float *pfQuat[3] = { &x, &y, &z };
		*pfQuat[i] = 0.5f * fRoot;
		fRoot = 0.5f / fRoot;
		w = (Mat.m[k][j] - Mat.m[j][k]) * fRoot;
		*pfQuat[j] = (Mat.m[j][i] + Mat.m[i][j]) * fRoot;
		*pfQuat[k] = (Mat.m[k][i] + Mat.m[i][k]) * fRoot;
	}
}

void Quaternion::ToMatrix(Mat4 &Mat)
{
	float fTx = 2 * x;
	float fTy = 2 * y;
	float fTz = 2 * z;
	float fTwx = fTx * w;
	float fTwy = fTy * w;
	float fTwz = fTz * w;
	float fTxx = fTx * x;
	float fTxy = fTy * x;
	float fTxz = fTz * x;
	float fTyy = fTy * y;
	float fTyz = fTz * y;
	float fTzz = fTz * z;

	Mat = Mat4(1 - (fTyy + fTzz), fTxy - fTwz, fTxz + fTwy, 0,
				fTxy + fTwz, 1 - (fTxx + fTzz), fTyz - fTwx, 0,
				fTxz - fTwy, fTyz + fTwx, 1 - (fTxx + fTyy), 0,
				0, 0, 0, 1);
}

void Quaternion::ToAngleAxis(float &fAngle, Vec3 &Axis)
{
	float fLengthSq = x * x + y * y + z * z;

	if (fLengthSq > EPSILONSQ)
	{
		fAngle = 2 * float(acos(w));
		Axis = v;
		Axis /= sqrt(fLengthSq);

	}
	else
	{
		fAngle = 0;
		Axis = Vec3(1, 0, 0);
	}
}

float Quaternion::Length()
{
	return sqrt(w * w + x * x + y * y + z * z);
}
float Quaternion::Norm()
{
	return w * w + x * x + y * y + z * z;
}
void Quaternion::Normalize()
{
	float fLen = Length();
	if (fLen == 0)
	{
		w = 1;
		x = y = z = 0;
	}
	else
	{
		fLen = 1 / fLen;
		w *= fLen;
		x *= fLen;
		y *= fLen;
		z *= fLen;
	}
}
void Quaternion::Invert()
{
	float fInvN = 1 / Norm();
	w *= fInvN;
	x *= fInvN;
	y *= fInvN;
	z *= fInvN;
}

Quaternion Quaternion::operator+(Quaternion &q)
{
	return Quaternion(w + q.w, x + q.x, y + q.y, z + q.z);
}
Quaternion Quaternion::operator-(Quaternion &q)
{
	return Quaternion(w - q.w, x - q.x, y - q.y, z - q.z);
}
Quaternion Quaternion::operator*(float f)
{
	return Quaternion(w * f, x * f, y * f, z * f);
}
Quaternion Quaternion::operator/(float f)
{
	float fInv = 1 / f;
	return Quaternion(w * fInv, x * fInv, y * fInv, z * fInv);
}
Quaternion Quaternion::operator*(Quaternion &q)
{
	Quaternion Temp = *this;
	Temp *= q;
	return Temp;
}
Quaternion Quaternion::operator+=(Quaternion &q)
{
	w += q.w;
	x += q.x;
	y += q.y;
	z += q.z;
	return *this;
}
Quaternion Quaternion::operator-=(Quaternion &q)
{
	w -= q.w;
	x -= q.x;
	y -= q.y;
	z -= q.z;
	return *this;
}
Quaternion Quaternion::operator*=(float f)
{
	w *= f;
	x *= f;
	y *= f;
	z *= f;
	return *this;
}
Quaternion Quaternion::operator/=(float f)
{
	float fInv = 1 / f;
	w *= fInv;
	x *= fInv;
	y *= fInv;
	z *= fInv;
	return *this;
}
Quaternion Quaternion::operator*=(Quaternion &q)
{
	float nw = w * q.w - x * q.x - y * q.y - z * q.z;
	float nx = w * q.x + x * q.w + y * q.z - z * q.y;
	float ny = w * q.y - x * q.z + y * q.w + z * q.x;
	float nz = w * q.z + x * q.y - y * q.x + z * q.w;
	w = nw;
	x = nx;
	y = ny;
	z = nz;
	return *this;
}
bool Quaternion::operator==(Quaternion &q)
{
	if (w == q.w && x == q.x && y == q.y && z == q.z)
		return true;
	return false;
}
bool Quaternion::operator!=(Quaternion &q)
{
	return !operator==(q);
}
Quaternion Quaternion::operator-()
{
	w = -w;
	x = -x;
	y = -y;
	z = -z;
	return *this;
}

Quaternion operator*(float f, Quaternion &q)
{
	return q * f;
}

float Dot(Quaternion &q1, Quaternion &q2)
{
	return q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
}
Quaternion Slerp(Quaternion &q1, Quaternion &q2, float fDelta)
{
	float fCos = Dot(q1, q2);
	float fFlip = 1;
	if (fCos < 0)
	{
		fCos = -fCos;
		fFlip = -1;
	}

	if ((1 - fCos) < EPSILON)
		return q1 * (1 - fDelta) + q2 * (fDelta * fFlip);

	float fTheta = float(acos(fCos));
	float fSin = float(sin(fTheta));
	float fBeta = float(sin((1 - fDelta) * fTheta)) / fSin;
	float fAlpha = float(sin(fDelta * fTheta)) / fSin * fFlip;

	return q1 * fBeta + q2 * fAlpha;
}